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Abstract 



The path planning problem, i.e. the geometrical problem of finding a collision-free path 
between two given configurations of a robot moving among obstacles, has been studied by 
many authors in recent years. The interest in constrained motion planning is more recent. 
Constrained motion planning consists in finding motion sequences for robotic systems whose 
free space in not an open subset of the configuration space. In particular, manipulation 
planning is an important instance of the general constrained motion planning problem. It 
consists in planning the motions of a system including robots and movable bodies that are 
constrained by grasping relationships. In a manipulation planning problem, the dimension of 
the free space may be dynamically changing along the solution path. 

In this report, we establish necessary and sufficient conditions under which grasping constraints 
are holonomic. Then we present a systematic approach to motion planning in the presence of 
grasping constraints deriving from this theory. Its principle is to replace a constrained problem 
by a convergent series of less constrained subproblems increasingly penalizing motions that do 
not satisfy the constraints. Each subproblem is solved using a standard path planner. We use the 
method of Variational Dynamic Programming for solving the subproblems. We report several 
experiments in manipulation planning with multiple redundant robots and multiple moving 
objects in configuration spaces having up to 12 degrees of freedom (DOF). The implemented 
planner has solved manipulation planning problems of unprecedented complexity. 
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Resume 



Le probleme de la planification de trajectoire, i.e. the probleme geometrique consistant a trouver 
des chemins sans collisions entre deux configurations d'un robot en presence d'obstacles, a 
ete largement etudie ces dernieres annees. L'interet pour les problemes de planification sous 
contraintes est plus recent. La planification de mouvement sous contraintes consiste a trouver 
des sequences de mouvements pour des systemes robotises dont l'espace libre n'est pas un sous 
espace ouvert de l'espace des configurations. En particulier, le probleme de la planification 
de manipulation est une instance importante du probleme general de la planification sous 
contraintes. II consiste a planifier les mouvements d'un systeme incluant des robots et des 
objets a manipuler lies entre eux par des contraintes de prehension. Dans un probleme de 
manipulation, la dimension de l'espace libre peut changer dynamiquement le long du chemin 
solution. 

Dans ce rapport, nous etablissons les conditions necessaires et suffisantes sous lesquelles une 
contrainte de prehension est holonome. Nous presentons ensuite une approche systematique 
pour la planification sous contraintes de prehension derivant de cette theorie. Son principe 
est de remplacer un probleme sous contraintes par une suite convergeante de sous-problemes 
moins contraints penalisant de facon croissante les mouvements qui ne satisfont pas les 
contraintes. Chaque sous-probleme est resolu grace a un planificateur de trajectoire classique. 
Nous utilisons la methode de Programmation Dynamique Variationnelle pour resoudre les 
sous-problemes. Des simulations effectuees sur plusieurs problemes de manipulation sont 
presentees dans ce rapport, incluant plusieurs robots redondants et plusieurs objets a manipuler 
dans des espaces ayant jusqu'a 12 degres de liberte. Le planificateur peut resoudre des 
problemes de manipulation d'une complexite sans precedent. 
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1 Introduction 

We present a systematic approach to constrained motion planning for robotic systems with 
many degrees of freedom (DOF). Constrained motion planning consists in finding motion 
sequences for robotic systems whose free space in not an open subset of the configuration space. 
In general, constraints in motion planning problems can be classified into two categories: 

• Path-independent constraints, i.e. constraints that only depend on the current configu- 
ration of the robot. Path-independent constraints are often called holonomic constraints. 

• Path-dependent constraints, that depend on more than a single configuration along the 
path. Path-dependent constraints are generally called nonholonomic constraints. An 
important class of path-dependent constraints is that of constraints on the first derivative 
(velocity) that are not integrable. 

In particular, manipulation planning is an important instance of the general constrained 
motion planning problem. Given an environment containing robots, stationary obstacles, and 
movable bodies, the manipulation problem consists in finding a sequence of free robot motions, 
grasping and ungrasping operations, to reach a given state from a given initial state in the joint 
configuration space of all robots and movable bodies. In a manipulation planning problem, 
the dimension of the free space may be dynamically changing along the solution path. 

We first develop a theory of manipulation planning. We show that in general a grasping 
constraint is nonholonomic, since it involves the first derivative of the path followed by the 
movable objects. We characterize conditions under which grasping constraints can be made 
holonomic. Loosely stated, a grasping constraint is holonomic if and only if the set of 
stable configurations for the movable object is a series of disconnected points called docking 
positions. 

Then, we present an implemented planner derived from this theory. This planner is capable of 
planning manipulation tasks when the number of docking positions is finite. This simplification 
makes the problem holonomic. The principle of our approach is to replace the original equality- 
constrained problem by a convergent series of more and more difficult inequality-constrained 
planning problems with open free spaces increasingly penalizing motions that do not satisfy 
the grasping constraints. We call this approach a penalty function method. In other words, 
grasping constraints are handled by our planner in a progressive fashion. We first solve the 
problem assuming that the movable objects can move without being grasped by a robot. Then, 
this path is used as an input for a series of more and more difficult problems forcing the objects 
to get closer and closer to the robots in order to move. Each subproblem is solved using 
a standard path planner. We use the method of Variational Dynamic Programming (VDP) 
described in Barraquand and Ferbach 1993 [2] for solving the subproblems. However, in 
theory, any other variational planner could be used instead of VDP. We call the overall method 
Progressive Variational Dynamic Programming (PVDP). 

The planner has successfully solved manipulation planning problems of unprecedented 
complexity. We report several manipulation planning experiments for systems with up 
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to 12 DOE 

Initially, this penalty function approach was developed in Barraquand and Ferbach 1993 [2] as 
a variant of the VDP method for solving difficult instances of the basic path planning problem 
in open free space. Indeed, given any variational path planning method, one can replace the 
collision avoidance constraints in a classical path planning problem by a convergent series 
of simpler constraints increasingly penalizing motions that do not satisfy collision avoidance 
constraints. The resulting planner is less general in theory than the original VDP planner, since 
it uses problem-specific heuristics to guide the search. On the other hand, it is dramatically 
faster. In fact, it can solve some problems in a time comparable to that of potential-field based 
methods (see Barraquand and Ferbach 1993 [2]). 

This report is organized as follows. In Section 2, we relate our contribution to previous work in 
motion planning. In Section 3, we develop a theory of manipulation planning, and characterize 
in particular conditions under which grasping constraints can be made holonomic. In Section 
4 we describe the general principle underlying the penalty function approach to manipulation 
planning in the presence of holonomic grasping constraints. We also describe our implemented 
planner PVDP In Section 5, we present experimental results illustrating the capabilities of the 
implemented planner. In section 6, we discuss current limitations and possible generalizations 
of the PVDP approach to manipulation planning. We also review possible applications of 
the penalty function method to other constrained motion planning problems. Section 7 is the 
conclusion. 

2 Relation to other work 

The path planning problem, i.e. the geometrical problem of finding a collision-free path 
between two given configurations of a robot moving among obstacles, has been studied by 
many authors in recent years (Latombe 1990 [8]). 

The interest in constrained motion planning is more recent in the robotics literature. The 
problem of planning the path of a convex polygonal robot translating in a two-dimensional 
polygonal workspace in the presence of multiple convex polygonal movable objects is 
addressed in Wilfong 1988 [12]. The general manipulation problem is described in a series 
of papers from Alami, Laumond, and Simeon (e.g. Alami Simeon and Laumond 1989 [1], 
Laumond and Alami 1989 [10]). 

To the best of our knowledge, although nonholonomic rolling constraints in motion planning 
have been investigated by several authors (e.g. Laumond 1986 [9] and subsequent papers, Li 
and Canny 1989 [11], Barraquand and Latombe 1989 [4], 1993 [6]), the nonholonomic nature 
of grasping constraints in manipulation tasks has never been investigated to date. 

An implemented algorithm for manipulation task planning with a 2 DOF robot grasping a 
single object at a time and several 2 DOF bodies translating in the plane is presented in Alami 
Simeon and Laumond 1989 [1]. The planner has two components: a classical path planner, and 
a manipulation task planner (MTP). The MTP plans a sequence a robot motions, grasping and 
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ungrasping operations, and transfer motions (i.e. motions of the robot together with a grasped 
object). The approach is in practice limited to non-redundant robots with few DOF, and 
requires an exhaustive exploration of the robot's configuration space. Koga and Latombe 1992 
[7] present several implemented planners solving various dual-arm manipulation planning 
problems of increasing difficulty. They use and extend the framework of Alami Simeon and 
Laumond 1989 [1]. The planner is again the combination of a path planner and a manipulation 
task planner. For problems with many degrees of freedom, the path planner used is the 
potential field based planner RPP (Barraquand and Latombe 1991 [5]). 

Our approach to manipulation task planning is fundamentally different. We do not decompose 
the problem into a sequence of robot motions and manipulation tasks. Our planner is not a 
combination of a path planner and a manipulation task planner. Instead, we simply consider 
the whole manipulation problem as a special instance of the basic path planning problem in the 
joint configuration space of the robot and the movable objects. The major advantage of this 
approach is to avoid the artificial decoupling between motion planning and task planning. As a 
consequence, PVDP can solve manipulation planning problems of unprecedented complexity. 

3 Manipulation tasks, grasping, and nonholonomy 
3.1 The manipulation planning problem 

We consider one or more robots R\,... ,Ri with respective configuration spaces C Rl ,C Rl , 
one or more movable objects Mi,.. . , M s with respective configuration spaces C Ml , C Ms , 
evolving in a workspace W populated by stationary obstacles. We assume that for any object 
A G {Ri , . . . , Ri, Mi , . . . , M s }, the manifold C A is bounded (hence compact, since it is a closed 
subset of the Euclidean space) and connected. The joint configuration space of the robots is 
denoted by C wbots = f]Li C R '■ Similarly, the joint configuration space of the movable objects 
in denoted by C ob i = 11/= l C Mj . The joint configuration space of the robots and movable 
objects is simply the cartesian product C = C wbots X C oh L We denote its dimension, i.e. the 
total number of degrees of freedom of all the robots and movable objects, by n. 

For any robot or movable object A £ {R\ , . . . , 7?/, Mi , . . . , M s }, we can define the projection 
that maps any configuration for all robots and objects q = (q^ 1 , . . . , q Rl , q Ml , . . . , q M <) £ C to 
the corresponding configuration q A £ C A of A. 

X : C — > C A 

q i— > 7r A (q) = q A 

We assume that an appropriate distance metric <f (q, q') has been defined over C. The collision 
avoidance space, i.e. the set of configurations q £ C such that robots, movable objects, and 
stationary obstacles do not collide with each other in denoted by C avo id- We assume that C avo id 
can be represented as 

Cavoid = {qeC, gavoid(q) < 0} 

where g aV oid is an appropriate function defined over C. For example, g aV oid could be the 
opposite of the minimum distance between any robot or object with other robots, objects, or 
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with obstacles. We see that the collision avoidance constraint is an inequality constraint in 
configuration space. C avo jd is an open subset of C. 

Let qtnit be the initial configuration of the system, and q goa i its desired configuration. The 
problem of manipulation planning consists in finding a sequence of free robot motions, 
grasping and ungrasping operations, to reach q goa i from q,„ lf in the joint configuration space C 
of the robot and all movable bodies. 

This sequence can be simply represented by a path 7 in C joining 7(0) = q ; „ !f and 7 ( 1 ) = q goa i- 
As in any classical path planning problem, the path 7 must not cross colliding configurations, 
i.e. we must have: 

Vf G [0, 1], 7 (» G C avoid 

However, there are two other kinds of constraints that must also be satisfied by 7 : grasping 
constraints, and stability constraints. These constraints are due to external forces (e.g. 
gravitational forces) applied to the movable objects. First, the movable objects can only 
move when they are grasped by some robot. Hence, the forbidden paths 7 are not only those 
for which the robots or the movable objects hit the stationary obstacles or collide with each 
other, but also all those for which the movable objects are moving without being grasped by 
a robot. Second, movable objects can be ungrasped by robots only within the subset of stable 
configurations, i.e. the subset of configurations where exterior forces sum to zero. 

The formalisation of these notions is the object of the next subsection. 
3.2 Grasping and stability constraints 

We first introduce the notion of grasping constraint by a simple example. More complex 
examples are presented in Section (5). Let us consider a 2-dimensional workspace W, with 
a single serial robot manipulator R with n ro j, ot = 2 revolute joints a\ and ai as illustrated 
in Figure (1). We assume that the robot base is fixed at the position (xo,yo) in W. The 
coordinates (x e ff,yeff) of the end-effector satisfy the forward kinematic equations: 

x e ff{a\, ai) = xo + L\ cos ai + Li cos ai 

y e ff{ai,a 2 ) = vo + Z4 sinai +L 2 sina 2 

where L\ and Lj_ are the respective lengths of the two robot arms. A configuration of R is 
a couple q R = (a\,ai). We assume that the workspace is populated by a single movable 
disk M with n 0 bj = 2 DOF translating in the plane, and whose configuration is defined by the 
coordinates of its center q M = (x m ,y m ). The total dimension of the joint configuration space 
is n = n ro bot + n Q bj = 4. We define a grasp between R and M as the set G grasp of configurations 
q = (ai,a2,x m ,y m ) of the whole system verifying: 

8 g rasp(®l ) a 2i X m , V m ) — X e ff — X m — 0 

and 

8 2 g rasp{ a ua2,x m ,y m ) = y eS - y m = 0 
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Figure 1 : A simple two-dimensional grasping problem in a two-dimensional configuration 
space 

The number h of coordinates of the grasping constraint g grasp = (g\ rasp , g^raspY is h = 2. 
We say that g grasp is a h = 2-dimensional grasping constraint. Hence, we see that G grasp is 
an - h = 2-dimensional submanifold of C. The dimension of a typical grasping constraint 
for a classical industrial robot with n ro h ot = 6 DOF manipulating a solid object with n 0 j,j = 6 
DOF in a three dimensional configuration space is h = 6. A robot with n ro b ot DOF designed 
for satisfying grasping constraints of dimension h = n 0 bj, i.e. for grasping objects with n 0 bj 
DOF, is said to be non-redundant iff n ro b ot = h = n 0 bj. The robot is said to be redundant iff 

n robot > h = n 0 f)j. 

More generally, given a robot /?, and an object My, an /i-dimensional grasping constraint gfrasp 
is represented by the n - h dimensional set of valid grasping configurations Gr^Mj'- 

G RuMj = {q G C, fSSCx^qJ.x^q)) = 0} 
where gfrosp is a continuous mapping from C Ri X C M; onto R h . 
g R g ^ : C R 'XC^ — a* 

(q*q«0 _ ^(q* > q«0=(^V.q M 0.---.^V.q M 0) r 

In general, a robot can grasp a given object in several different ways. For example, if the robot 
at hand has two arms, and the object to be manipulated is a long bar, there are two possible 
ways of grasping the long bar using both arms at both extremities of the bar, that correspond 
to the possibility of swapping the two arms. Such problems are called dual arm manipulation 
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planning problems. An example of such problem is presented in Section (5). Also, any finite 
set of robots can be viewed as another robot. Hence, the above formalism applies to complex 
problems such as multifmgered manipulation planning. We take the set Gr u m as the set of all 
possible valid grasping configurations, regardless of the way the object is being grasped. 

A manipulation path satisfies the grasping constraints iff for any movable object Mj, the object 
is either stationary or grasped by some robot Rj. Formally: 

we [0,1], (a/G[i,/], «SS(x*( 7 (0),^(7(0)) = o) V (|^(t(0) = o) (i) 

We see that a grasping constraint may be nonholonomic, since it involves the first derivative 
of the path followed by the movable object. We will formally prove this intuitive fact in the 
next subsection. Before, we introduce a simplified concept of stability. 

An object Mj can be ungrasped by some robot only if its configuration q M J is stable with 
respect to exterior forces, e.g. with respect to the gravitational force and reaction forces from 
stationary obstacles. It may be that the configuration of other objects influences the stability 
of Mj. Here we only consider absolute stability, i.e. stability in the absence of other objects. 
Hence, the set of stable configurations of Mj is a well-defined subset of C M >. It is denoted by 
STABLE (My). We assume that STABLE(M ; ) is a closed set. 

3.3 Nonholonomy of grasping constraints 
3.3. 1 An introductory example 

We first consider the example above, and then turn to the general case. Applying formula (1) 
to the example of Figure (1), we get: 

we [0,1], (%W-^)=mW-^)) = o)V(^W = ^W = °) ( 2 ) 

If the above two-dimensional constraint were holonomic, we could integrate it, i.e. 
we could find a couple of real-valued functions F\ and F 2 such that a path *y(t) = 
(ai(t),a 2 (t),x m (t),y m (t)) satisfies the constraint iff 

Vf G [0,1], F 1 (a 1 (t),a 2 (t),x m (t),y m (t)) = F 2 (a 1 (t),a 2 (t),x m (t),y m (t)) = 0 

If we define the real-valued function F = \Jf\+F\ = ||(Fi,F2) r || R 2, the above constraint 
can be rewritten: 

Vf G [0, 1], F( ai (t),a 2 (t),x m (t),y m (t)) = 0 

We consider an arbitrary object location (x^,v°) reachable by the robot, i.e. such that 
there exists (a®, a®) verifying x e ff{a\,aj) = x° and y e ff{a°\i a 2 ) = Hi- We write (x° , v°) G 
REACH(m). We consider a path starting at ai(0) = a®, a 2 (o) = a2,x m (o) = x„,y m (o) = 
v° consisting of immediately ungrasping the object and bringing the robot to an arbitrary other 
location a\ = ai(l), a\ = 02(1)- Along such a path, the object will not move since it must 
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satisfy (2). Hence, we have x m {\) = x Q m and y m (l) = y Q m . By holonomy, we must have at 
t = 1: 

We have just shown that for any reachable couple {x% ,y% ), the function F takes the value 0 no 
matter the value taken by its first two variables a\ and a\: 

y(a\,a\)eC R , V{x° m ,y° m ) G REACH (m), F(aJ,ai,x° ,y° m ) = 0 

Hence, F is identically null over the set of reachable configurations. Now, given any 
configuration {a\,a\,x^,y^) such that (x%,y%) is reachable, we take any other reachable 
configuration (x^,y^) different from (x° ,y° ), and we can consider the path: 

Vre[0,l], ai(f)=fli, a 2 (0=«2. x m (t) =x° m + t(x l m -x° m ), y m (t) =y° m + t(y l m -y° m ) 

Along this path, F is identically null by the above result. But this path obviously does not 
satisfy constraint (2). Hence, we have a contradiction. The constraint (2) cannot be holonomic. 

3.3.2 General case 

We now turn to the general case. Before stating the result, we first define formally the notion 
of reachable configuration for a movable object. Given an object Mj, the configuration q M ' is 
reachable and we write q M J £ REAC^M,) iff there exists a robot 7?, that can grasp Mj at q M J . 
The set of reachable configurations REACH {Mj) can be defined as follows. 

REACH(My) = {<f> G C M >, 3i 6 [1, /], 3q R - 6 C R ' , g^(q R ', q M >) = 0} 

Clearly, REACH(M 7 ) is closed, since the constraints gf r '^p are continuous and C is compact. 
We call docking position an object configuration that is both stable and reachable, and we write 

dock(m,) = stable(m,) n reach(m ; ) 

By the above results, we see that D0CK(M 7 ) is a closed bounded subset of C M K Hence, 
D0CK(M 7 ) is compact. We will prove the following result. 

Characterization of holonomic grasps 

We consider the grasping constraint (1) for an object Mj. The following two 
statements are equivalent. 

i) All pathwise-connected components of the set of stable reachable object positions 
DOCK{Mj) = STABLE{Mj) n REACH{Mj) are singletons. 

ii) The constraint is holonomic 

In other words, a grasping constraint is holonomic iff there is no path composed only of stable 
reachable configurations connecting two different stable reachable configurations. 
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Proof of ii) => i). 

We first prove ii) => i) by generalizing the argument developed above for the example in 
Figure (1). 

Assume the constraint (1) is holonomic. We consider a function F mapping C onto R such that 
(1) is equivalent to: 

Vf G [0, 1], F( 7 (0) = 0 
We take an arbitrary q 0 ; G DOCK(A/,-J. Since q 0 ; is reachable, there exists a robot 7?, and a 

p. 

configuration q 0 ' such that 

W(q 0 ',q 0 ; ) = 0 

Without loss of generality, we can consider all other objects and robots as static, and limit our 
study to paths in C Ri X C M J. We define the path 7 starting at 7(0) = consisting 

p. 

of ungrasping Mj and bringing /?, to an arbitrary other location q^. This path is valid since 
q 0 J is a stable configuration. By the grasping constraint (1) we must have 7(1 J = (q^, q 0 ; J. 
By holonomy, we have F(qf , q 0 J ) = 0. Since C Ri is connected, this is true for any couple 
(qf , <& J ) G C R - X DOCK(M ; ). We can write: 

F{C R ' X DOCK(My)) = 0 (3) 

Now, we take any other object configuration qf J G D0CK(M 7 ). We will show that there can 
be no path 8 in D0CK(M 7 ) linking #(o) = q 0 1 and #(l) = q x 1 . This will in turn prove 1). 
Indeed, if such a path exists, we can define the path 7(f) = (q Ri (t) , q Mj (t)) in C Ri X C M '\ 

WG[0,1], q /? '(0 = qf, q M '{t) = b{t) 

By (3) and //), this path 7 satisfies the grasping constraint. But clearly 7 cannot satisfy (1) 
since S joins two different configurations. Hence S cannot exist, and we get the desired result. 

Proof of i) => ii). 

Reciprocally, let us assume i). We define the following real-valued function over C: 

F(q)=mm(m 1 n(||^(^(q),^(q))||), mm (d(^(q),D))) (4) 

Since D0CK(M 7 ) is a compact set, the minimum m i n £) e DOCK(M ) ( < ^( 7rMj ( < l)'^)) ^ s wei ^" 
defined by the Bolzano-Weierstrass theorem. Hence, F(q) is a well-defined function. 

We will show that the grasping constraint (1) is equivalent to the following holonomic 
constraint: 

Vt G [0, 1], F( 7 (0) = 0 (5) 
Indeed, the above holonomic constraint can be rewritten: 

V?G[0,1], (3iG[l,/], 8 R ^^ R il{t))^ M '{l{t))) = 0) V 

(3D(t) G DOCK(M,), v M j{l{t)) = D(t)) 
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Hence, it suffices to show the following implication for any interval [t\ , t{\ C [0, 1] 

(Vf G [t u t 2 ], 3D(t) G DOCK(M y ), ^ M '{j{t)) = D(tj) 

= > (VfGEfi.ft], |^( 7 (0) = o) 

But, since DOCK(A^) verifies i), the path D(t) is necessarily constant: 

Vte[t u t 2 ], * M ii{t)) = D(t) = D 

Deriving the above identity with respect to t, we get the desired result. 
3.4 Docking positions and holonomic grasp planning 

In this paper, we consider only manipulation planning problems for which the grasping 
constraints are holonomic. From the above result, this implies that we impose to the set 
of stable reachable configurations (i.e. docking positions) to be composed of pathwise- 
connected components containing a single element. In practice, we will restrict ourselves to 
problems for which a movable object Mj is only stable at a finite number of docking positions 
Vm G [1 , dj],Dj G C Mj , unless it is grasped by a robot. Formally: 

DOCK(M y ) = {£>],..., £0 

We call free space and denote by Cf ree the subset of C avo id satisfying the (holonomic) grasping 
constraints. We see that in general, Cf ree is not an open subset of C, since the grasping 
constraints are equality constraints. The dimension of Cf ree can dynamically change along the 
solution path. This may happen when a robot grasps an object (reduction of the dimension) 
or ungrasps it (increase in the dimension). Hence, a standard path planner only capable 
of planning motions in open free spaces of constant dimension cannot be used for solving 
the manipulation planning problem. In the next section, we introduce a new paradigm for 
circumventing this limitation of path planning techniques. 

4 A penalty function method for holonomic manipulation planning 

In this section, we present a new paradigm for solving motion planning problems in the 
presence of holonomic constraints. This approach applies in particular to manipulation 
planning problems when the set of docking positions for movable objects is finite and 
prespecified as part of the planning problem. 

4.1 An introductory example 

We first illustrate the approach on a manipulation planning example using the setup of Figure 
(1). We assume that initially the robot R is at the configuration (af", af") = (a®, a®), and 
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that the object M is located at an arbitrary reachable configuration (x 1 ™' , = (-^mj^m)- 
We define the goal configuration for the robot as (af" 1 , af" 1 ) = (a®, a®), and the goal 
configuration for the object as (xff a/ , yf? a ') = {x m ,yj„) 7^= {xm,y m )- We see that the task 
assigned to the robot is simply a pick and place operation consisting of moving M to another 
location. Hence, in this case, it is easy to imagine how the problem should be addressed. First, 
the robot must move its end effector towards the object and grasp it. Second, both the robot and 
the grasped object must move in order to bring the end-effector (hence the object) to the goal 
configuration. Third, the robot must ungrasp the object and get back to its initial configuration. 
The above manipulation problem can be decomposed into a sequence of three basic motion 
planning problems in open free space. One may argue that it is not necessary to develop 
planners capable of dealing with grasping constraints, since manipulation problems can be 
decomposed into sequences of classical path planning problems without grasping constraints. 
However, the task of planning the sequence of motions, called manipulation task planning, is 
often much more complex than in the above pick and place problem. For example, there can 
be more than one object to manipulate, and the geometry of the workspace may imply that 
different objects interact with each other. As another example, the object to manipulate can 
be a long bar that must be grasped at the same time by two robot arms. In such a case, the 
robot arms may have to switch their respective grasps at the two ends of the bar. Examples 
of such problems will be presented in Section (5). For those more difficult problems, the 
task of planning the sequence of motions, grasping, and ungrasping operations may become 
a hard problem in itself. Then, a manipulation planner should be the combination of two 
planners: 1) a classical path planner in open free space computing motions between grasping 
and ungrasping operations 2) a manipulation task planner computing the sequence of grasps. 
This is the way the problem has been addressed in previous work (see Section (2)). 

Here, we take an opposite approach, and attempt to solve the whole manipulation planning at 
once. This enables us to deal with intricated interactions between geometric and task planning 
constraints that cannot be taken into account by other planners decoupling the two problems. 

We first define two docking positions for the object M, namely its initial and goal positions. 



In a more difficult problem, we might have to define intermediate docking positions. Examples 
of such intermediate docking positions are given in Section (5). This raises the problem of 
defining these docking positions as part of the planning task. Although the issue is still open 
in the general case, we briefly discuss it in Section (6). 

Following the results of the previous section, the grasping constraint can be replaced by the 
following scalar holonomic constraint: 



DOCK(M) = {{x° m ,y\ 



'm) ) \ x mi ym 



)} 



Vf G [0, 1], F(a 1 (i),a 2 {t),x m (i),y m (i)) = 0 



with 



F , #2) Xm: ym) 



min y (x eff -x m ) 2 + (y eff - y m ) 



2 , min J(. 
«e{i,2} v v 



X U mY + {ym - ym) 1 
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We define a decreasing sequence of positive numbers ek,k > 0 converging towards 0. For 
example, we can choose e& = 1 /k. We replace the pick and place problem P defined above by 
the sequence of problems P# for which the grasping constraint is replaced by: 

Vf G [0, 1], F( ai (t),a 2 (t),x m (t),y m (t)) < e k 

We see that the free space for problem P& is an open subset of the configuration space. Hence, 
we can use a standard path planner to solve P&. 

The idea underlying the penalty function method is that, since the difficulty of solving P# 
increases with k, it is better to first solve problem Pi , and then use the solution path of problem 
Pi as a heuristic to solve problem Pi- hi turn, the solution hopefully obtained of problem P2 
can be used as an input heuristic for solving problem P3 , and so on until the grasping constraint 
is satisfied up to a prespecified accuracy €k max = 1 /k max . 

Hence, the penalty function method will work well if one can devise a path planner that makes 
efficient use of the solution path of problem Pu-\ in order to solve problem P&. Such a path 
planner is called a variational path planner. Variational planning is discussed in the next 
subsection. 

4.2 Variational planning 

Variational path planning consists of iteratively improving an initial path possibly colliding 
with obstacles (or any other inequality constraint) in order to obtain a collision free path. 

We use the method of Variational Dynamic Programming (VDP) developed by Barraquand and 
Ferbach 1993 [2]. VDP consists in perturbing at each iteration the current path by performing 
a dynamic programming search around the current path in a m-dimensional submanifold of 
the n-dimensional configuration space C. In practice, m is chosen equal to 2,3, or 4, in order 
to make the dynamic programming search tractable. The m-dimensional submanifold is an 
arbitrarily chosen ruled surface containing the current path. This surface is quantized into a 
m-dimensional grid of configurations. Then, the grid is searched using Dijkstra's algorithm 
with an additive cost function proportional to the number of configurations colliding with 
obstacles. Hence, the new path obtained is guaranteed to collide obstacles less than the 
previous path. Then, the operation is repeated until a free path is found. See Barraquand and 
Ferbach 1993 [2] for more detail on the VDP algorithm. 

Experiments reported in Section 5 show that VDP is well suited to using the output solution 
of problem P&_i in order to solve efficiently problem P&. However, other planners capable of 
planning paths in high dimensional configuration spaces could be adapted in theory to imbed 
the same feature. For example, the RPP planner described in Barraquand and Latombe 1991 
[5] is not a variational planner in its original form, but could be adapted in the following 
fashion. 

Given a solution path ji c _ 1 of problem P&_ 1 , and since the solution of problem P\ should not 
differ significantly from j k _ 1 , one could estimate the maximum distance d max over all t 6 [0,1] 
between configuration 7^-1 (t) and the corresponding configuration Jk{t) on a solution path 
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for problem Then, 7^_i could be used to include the additional heuristic constraint on the 
free configuration space Cp. ee of problem P>: 

Vq G Cf ree , 3t G [0, l],c?(q,7<fc-i(0) < 

This additional heuristic constraint would force the path planner RPP to limit its search for a 
solution path 7^ of problem Pt to a "tubular neighborhood" of the previous solution path jk-i- 
We plan to investigate this alternative in future research. 

4.3 General case 

We consider the general manipulation planning problem of Section (3). We assume a finite 
number of docking positions for the objects have been prespecified as part of the planning 
problem. From the results of subsection (3.3), and more specifically from formula (4), we see 
that the grasping constraints for object Mj can be expressed: 

v* g [0, l], fXt(O) = 0 

with 

F,(q) = mm (min (||^(^(q),^(q))||) (</(*"'(q),D))) 

For each object Mj, we can choose a positive decreasing sequence of numbers d k converging 
towards zero, an replace our original manipulation planning problem P by a sequence of 
problems P^ using the partially relaxed grasping constraints: 

V/G [1,5], V/G [0,1], Fj( lk (t)) < 4 

The penalty function method consists in first solving the simpler problem Pi as a standard path 
planning problem in open free space using a variational path planner such as VDP, to obtain a 
first path 7! . Then, for any k > 1 , problem Pt is solved using as input to the variational planner 
the solution j/ c _ 1 of the previous problem Pt- 1 . The algorithm is stopped when the numbers 
Ek = (4, . . . , e|) are all smaller than a prespecified tolerance value ek max - The overall method 
is called Progressive Variational Dynamic Programming (PVDP), since the penalties on the 
grasping constraints are applied in a progressive fashion. 

The vector sequence (Ek)k>o is called e-strategy. Both the rate of convergence towards 0 and 
the relative values for a given k of the various numbers e| , . . . , e s k may influence the overall 
computation time and even the nature of the solution found by the planner. 

More complex e-strategies can be devised. Indeed, many different functions Fj can be used 
to represent the same holonomic grasping constraints applying to the object Mj. For example, 
when the dimension h of a given grasp between robot R t and object Mj is higher than 1, one 
may choose any other equivalent norm instead of the standard L 2 Euclidean norm to compute 
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the number ||gfrasp||. One may find it preferable to use a weighted L 2 norm instead of the 
standard norm: 



(\T\ i weighted 
gl,---,gh) \\ L 2 = 



where ai, . . . , a/, are adequately defined positive numbers depending on the type of grasp 
considered. Alternatively, one may use the weighted L°° norm: 

1 1 / \ 7* 1 1 weighted i i 

\\{gi,---,gh) Hl~ = max a t \ g t \ 

iE{l,h] 

Also, any other additional heuristic constraints can be added to improve the progressiveness in 
the difficulty of the sequence of problems when facing a particularly complex manipulation 
problem. In general we call e-strategy the whole set of empirical parameters that can be used 
to define the holonomic constraints Fj and their rate of convergence towards 0. Examples of 
efficient problem-specific strategies are presented in Section (5). 



5 Experimental results 

We have implemented PVDP in a program written in C, running on a DEC3000-500 Alpha 
AXP workstation. We have experimented with PVDP using a variety of manipulation planning 
problems. The most significant of these experiments are described below. 

5.1 10-DOF non-serial manipulator robot grasping a 2-DOF disk. 

We consider the planar non-serial manipulator robot depicted in Figure 2, which includes three 
prismatic joints (telescopic links) and seven revolute joints. The task assigned to this robot is a 
simple pick and place operation consisting in grasping the disk in the lower right corner of the 
workspace, bringing it to the lower left corner, and then returning to its initial configuration. 
The total number of degrees of freedom for the whole problem is 12. Figure 2 illustrates a 
manipulation plan found by PVDP. 

In this example, the robot is said to have grasped the disk when the following conditions are 
satisfied: 

• the center M of the disk coincides with the middle R = El+ ^ 2 of the two end-effectors 
E\ and Ei of the robot. 

• the distance | \E\E2 \ | between the two end effectors E\ and E2 is equal to the diameter D 
of the disk. 

There are two admissible docking positions Mi and Mi for the disk, namely its initial and goal 
configurations. 
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The grasping constraint Vf, F(j(t)) = 0 is replaced in the approximating problem P e by the 
constraint \/t, F(j(t)) < e with the following expression for ^(q). 



F(q) = min ( max(| |£i£ 2 | I - D,\\RM\ 



, min 1 1 MM, I 

(£{1,2} 



In other words, in problem P e , either the disk is at distance less than e of a docking position, 
or if satisfies both conditions | \E\E2 \ \ < D + e and \ \RM\ \ < e. 

We denote by F\E\ the segment representing one of the two terminal fingers of the robot, F2E2 
the other one. In addition to the e-strategy defined above, we use in problem P e the following 
heuristic constraint, systematically satisfied in the original definition of the grasp. If M is at 
distance longer than e from both M\ and M 2 , it must always lie in the domain of the workspace 
bounded by 1) the two straight lines prolongating the two robot fingers, i.e. the lines passing 
through F\,E\ and F 2 , E2, and 2) the line F1F2. This additional heuristic constraint enhances 
the progressiveness in the difficulty of problem P e . 

The initial value of e is one fourth of the size of the workspace. Then, it is decreased at each 
iteration of the penalty function method by 0.001, i.e. 0.1% of the size of the workspace. The 
tolerance value was set to ek max = 0.006, i.e 0.6% of the workspace. The path was computed 
in about half an hour. 
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Figure 2: A pick and place operation using a 10-DOF non-serial manipulator 
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5.2 3-DOF serial manipulator robot manipulating two 2-DOF disks. 

We consider the planar serial manipulator robot depicted in Figure 3, which includes two 
revolute joints and one telescopic link. The robot is composed of 1) a single telescopic arm, 
and 2) an end effector that can rotate at the end of the telescopic arm. The task assigned to this 
robot is to move both disks from the right-hand bucket to the left-hand bucket on the bottom 
of the workspace. Additionally, the two disks must be put in the left-hand bucket in a last-in 
first-out order, i.e. the disk on the top of the right-hand bucket at the initial configuration must 
be on the top of the left-hand bucket at the final configuration. Hence, the robot must first 
move the top disk to an intermediate docking position, then put the disk at the bottom of the 
right-hand bucket into the left-hand bucket, and finally bring the top disk from its intermediate 
position to the top of the left-hand bucket. 

The total number of degrees of freedom for the whole problem is 7. Figure 3 illustrates a 
manipulation plan found by PVDP. 

Let Ei and Ei be the two points at the extremities of the two fingers on the end-effector. Let M 
be the center of any one of the two disks. Let D be the diameter of the disks. In this example, 
the robot is said to have grasped a disk iff the distances E\M and EjM are both equal to D/2. 

There are three docking positions for the top disk: 1) the initial position, 2) the goal position, 
3) the intermediate docking position on top of the central obstacle separating the two buckets 
(see figure (3)). There are two docking positions for the bottom disk: 1) the initial position, 2) 
the goal position. 

The grasping constraints in the partially relaxed problems P e were defined in a fashion similar 
to that of the previous example. The total computation time in the example shown in figure 
(3) was 45 minutes. The planner PVDP was also capable of solving the same problem using 
a randomly selected intermediate docking position on top of the obstacles, instead of the 
prespecified position in the center (Figure 4). 

5.3 Two 3-DOF manipulator arms manipulating a 3-DOF bar 

We consider the dual-arm manipulating planning problem depicted in Figure 5. There are two 
identical 3-DOF arms. Each arm has two links. The first link has fixed length, and can rotate 
around the base. The second link is telescopic and has two degrees of freedom, one revolute 
and one prismatic. The bar has 3-DOF. The bar can only move when it is grasped by both 
arms. The task assigned to the two arms is to move the bar from the right side to the left 
side of the workspace. Because of the presence of an obstacle, the two arms must swap their 
respective grasps before reaching the goal. The total number of degrees of freedom for the 
whole problem is 9. Figure 5 illustrates a manipulation plan found by PVDP. The path was 
computed in about 40 minutes. 

The definitions of the grasping constraints and of the approximating problems P e are similar 
to that of the previous examples. Three docking positions are allowed for the bar: its initial 
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Figure 3: A 3-DOF arm manipulating two disks 

position, its goal position, and an intermediate vertical position in the middle of the workspace. 
The specification of this intermediate position is critical for the success of the manipulation 
plan. We have chosen this intermediate docking position in an ad-hoc fashion. If this 
intermediate position had been randomly chosen in the workspace, the planner would have 
most probably failed to find a manipulation plan. This example demonstrates that the choice 
of intermediate docking positions is a serious limitation of our manipulation planner in its 
current implementation. In Section (6), we briefly discuss how intermediate docking positions 
could be dynamically determined by the planner itself in a less ad-hoc fashion. 

5.4 Three robots manipulating a 2-DOF disk 

We consider the planar manipulation planning problem depicted in Figure 6. The robot on the 
lower left side is a kind of piston that can extend vertically. It has 1 DOF, which corresponds 
to the prismatic extension link. It can grasp the 2-DOF disk on the middle of the horizontal 
shelf. Another robot with 1 DOF on the upper left corner can only rotate around its base. The 
third manipulator is composed of one telescopic arm, another arm with fixed length and two 
revolute joints. This manipulator has 3 DOF. The task assigned to these manipulators is to 
move the 2-DOF disk from the right side to the left side of the picture. At the end the disk is 
placed on the piston in the lower left corner. 

The total number of degrees of freedom for the whole problem is 7. Figure 6 illustrates a 
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Figure 4: The same problem with a randomly chosen intermediate docking position 
manipulation plan found by PVDP in 13 minutes. 

This example shows a solution where several manipulators have to cooperate with each other: 
one robot picks the disk and brings it to a second manipulator that gives the object to a third 
one. The solution has been found without using any docking position or predefining the 
positions where the manipulators transmit the disk between each other. 

6 Discussion 

6.1 Current limitations and possible extensions 
6.1.1 Automatic selection of docking positions 

The examples presented above show that the penalty function method is capable of dealing 
with manipulation planning problems of unprecedented complexity. However, the method has 
a severe limitation in its current form: an appropriate set of docking positions must be chosen 
for each manipulation problem. Often, a simple and natural choice consists in considering only 
docking positions that correspond to the initial or goal configurations for the movable objects. 
But in some more complex problems, intermediate docking positions must be prespecified in a 
somewhat ad-hoc fashion. We believe this limitation of the current planning algorithm could 
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Figure 5: A dual arm manipulation planning problem 

be removed by denning the set of docking positions dynamically while the manipulation plan 
is constructed. Indeed, one could define the intermediate docking positions in the following 
way. Using the notations of Subsection (4.3), and given an object Mj, we can consider the 
grasping function: 

G y (q) = .min||^(^(q),^(q))|| 

!G[1,Z] 

Then, we can subdivide the current solution path 7^ of problem P k into a sequence 0 = to < 
h < ... < t2r< hr+i = 1 verifying: 

Vp G [0,r],Vf G [t 2 p,t2 P+ l],Gj(j k (t)) > 4 

and 

G [0, r — 1],V* e\hp +u t lp+ 2lGj( lk (t)) < 4 

In other words, we can subdivide the current path into subintervals where the object is 
successively close (i.e. at distance less than 4) to some robot end-effector, of far from any 
robot's end-effector. Then, for each subinterval [t2 p , t2 p +i] for which the object is far from 
all robots, we can define dynamically the intermediate docking position in the middle of the 
subinterval: 

fl/ =x ^( 7t (^Etl)) 

The r + 1 intermediate docking positions Dq, . . . , Dj. then defined can be used as the new set 
of docking positions for problem Pt+i- We plan to investigate this idea in future research. 
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Figure 6: A pick and place operation with three cooperating robots 



6.1.2 Computation of efficient e -strategies 

Besides the problem of defining intermediate docking positions, another important issue is 
to define an appropriate e-strategy. Although we have found that the planner is relatively 
robust with regards to the choice of the e-strategy, future research is needed to reduce the 
number of empirical parameters associated with the definition of the e-strategies. Devising 
efficient computational techniques for automatically selecting the appropriate strategies is an 
open problem. 

6.1.3 Combining the penalty function method with other path planners 

Finally, although the method of Variational Dynamic Programming has proven efficient for 
solving the subproblems P^, other variational planners could be used in theory to solve the 
same problem. In Subsection (4.2), we have briefly described how standard planners such 
as RPP capable of dealing with many degrees of freedom could possibly be adapted for this 
purpose. We believe this idea is also worth investigating. 
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6.2 Other applications of the penalty function method 

6.2. 1 Application to standard path planning problems 

Initially, this penalty function approach was developed in Barraquand and Ferbach 1993 [2] as 
a variant of the VDP method for solving difficult instances of the basic path planning problem 
in open free space. Indeed, given any variational path planning method, one can replace the 
collision avoidance constraints in a classical path planning problem by a convergent series 
of simpler constraints increasingly penalizing motions that do not satisfy collision avoidance 
constraints. The resulting planner is less general in theory than the original VDP planner, since 
it uses problem-specific heuristics to guide the search. On the other hand, it is dramatically 
faster. In fact, it can solve some problems in a time comparable to that of potential-field based 
methods (see Barraquand and Ferbach 1993 [2] for more detail). 

6.2.2 Applications in assembly planning 

More generally, the penalty function method can be used to represent any kind of holonomic 
constraints in motion planning. This opens a broad range of new possibilities. In particular, 
most constraints in assembly planning problems are holonomic equality constraints. Hence, an 
assembly planning problem can in theory be approximated by a sequence of simpler problems 
P e where the different objects to be assembled are allowed to overlap with each other by 
a distance not greater than e. Although we do not have investigated the application of the 
penalty function method to assembly planning, we feel that this line of research is particularly 
promising. 

7 Conclusion 

This report described a new approach to motion planning with holonomic constraints, which 
essentially consists of replacing a constrained problem by a convergent series of less constrained 
subproblems increasingly penalizing the motions that do not satisfy the constraints. Each 
subproblem is solved using a variational path planner. 

We have applied the approach to manipulation planning problems in the presence of holonomic 
grasping constraints. To this end, we have characterized the conditions under which grasping 
constraints can be made holonomic. In practice, a grasping constraint on a movable object is 
holonomic if the number of stable reachable configurations is finite. 

This approach has been implemented in a program, called PVDP, which was run successfully 
on several difficult manipulation planning problems. We used the method of Variational 
Dynamic Programming to solve the subproblems, although other variational planners could be 
used in theory. 

The penalty function method could be applied to any other kind of holonomic constraint. 
A very promising line of research is the application of this method to assembly planning 
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